#include "hardware_controller/base_controller.h"

namespace base_controller{

BaseController::BaseController():rclcpp::Node("hardware_controller"),serial_parser_(nullptr){
    this->declare_parameter<std::string>("port", "/dev/stam32_f4");
    this->declare_parameter<int>("baud", 115200);
    this->declare_parameter<std::string>("base_frame", "base_link");
    this->declare_parameter<std::string>("odom_frame", "odom");
    this->declare_parameter<std::string>("imu_frame", "imu_link");
    this->declare_parameter<bool>("odom_tf", true);

    this->get_parameter("port", serial_port_);
    this->get_parameter("baud", serial_baud_);
    this->get_parameter("base_frame", base_frame_);
    this->get_parameter("odom_frame", odom_frame_);
    this->get_parameter("imu_frame", imu_frame_);
    this->get_parameter("odom_tf", odom_tf_);
    RCLCPP_INFO(this->get_logger(), "port is %s, baud: %d, base_frame: %s, odom_frame: %s, imu_frame: %s, odom_tf: %d", 
        serial_port_.c_str(), serial_baud_, base_frame_.c_str(), odom_frame_.c_str(), imu_frame_.c_str(), odom_tf_);

    odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/car/odom", 10);
    imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("/car/imu", 10);
    voltage_pub_ = this->create_publisher<std_msgs::msg::Float32>("/power_voltage", 10);
    cmdvel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
        "/cmd_vel", 1, std::bind(&BaseController::cmdvel_cb, this, std::placeholders::_1));

    serial_parser_ = new SerialParser(serial_port_,serial_baud_);

    OdomReset();
    ControlCore();
}

BaseController::~BaseController(){
    serial_parser_->SetMotorSpeed(0.0f,0.0f,0.0f);
    serial_parser_->SerialClose();
    if(serial_parser_ != nullptr) 
        delete serial_parser_;
}

// 速度回调函数
void BaseController::cmdvel_cb(const geometry_msgs::msg::Twist::SharedPtr vel_msg){
    if(serial_parser_ != nullptr)
    // speed_time_last_ = ros::Time::now();
    speed_time_last_ = rclcpp::Node::now();
    serial_parser_->SetMotorSpeed(vel_msg->linear.x, vel_msg->linear.y, vel_msg->angular.z);
}

// 里程计重置
void BaseController::OdomReset(){
    robot_x_ = 0.0f;
    robot_y_ = 0.0f;
    robot_yaw_ = 0.0f;
}

/*
    发布里程计
*/
void BaseController::Publish_Odom(){
    // geometry_msgs::msg::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(robot_yaw_); //转换为四元数
    tf2::Quaternion q;
    q.setRPY(0,0,robot_yaw_);
    geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q);

    // 里程计构造
    nav_msgs::msg::Odometry odom_msg;
    // odom_msg.header.stamp = ros::Time::now(); 
    odom_msg.header.stamp = rclcpp::Node::now();
    odom_msg.header.frame_id = odom_frame_; 
    odom_msg.child_frame_id = base_frame_;
    odom_msg.pose.pose.position.x = robot_x_; 
    odom_msg.pose.pose.position.y = robot_y_;
    odom_msg.pose.pose.position.z = 0.0f;
    odom_msg.pose.pose.orientation = odom_quat; 
    odom_msg.twist.twist.linear.x =  serial_parser_->Robot_Vel.X; 
    odom_msg.twist.twist.linear.y =  serial_parser_->Robot_Vel.Y; 
    odom_msg.twist.twist.angular.z = serial_parser_->Robot_Vel.Z; 
    //这个矩阵有两种，分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵，用于robot_pose_ekf功能包
    if(serial_parser_->Robot_Vel.X == 0 && serial_parser_->Robot_Vel.Y == 0 && serial_parser_->Robot_Vel.Z == 0) //如果velocity是零，说明编码器的误差会比较小，认为编码器数据更可靠
    memcpy(&odom_msg.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)),
    memcpy(&odom_msg.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));
    else  //如果小车velocity非零，考虑到运动中编码器可能带来的滑动误差，认为imu的数据更可靠
    memcpy(&odom_msg.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)),
    memcpy(&odom_msg.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance));       
    odom_pub_->publish(odom_msg); 

    if(odom_tf_){
        // 提供坐标变换
        br_ =
            std::make_unique<tf2_ros::TransformBroadcaster>(*this);
        geometry_msgs::msg::TransformStamped transformStamped;
        // transformStamped.header.stamp = ros::Time::now();
        transformStamped.header.stamp = rclcpp::Node::now();
        transformStamped.header.frame_id = odom_frame_;
        transformStamped.child_frame_id = base_frame_;
        transformStamped.transform.translation.x = robot_x_;
        transformStamped.transform.translation.y = robot_y_;
        transformStamped.transform.translation.z = 0.0f;
        tf2::Quaternion q_tf;
        // tf2::fromMsg(odom_quat, q_tf);
        q_tf.setRPY(0, 0, robot_yaw_);
        transformStamped.transform.rotation.x = q_tf.x();
        transformStamped.transform.rotation.y = q_tf.y();
        transformStamped.transform.rotation.z = q_tf.z();
        transformStamped.transform.rotation.w = q_tf.w();
        br_->sendTransform(transformStamped);
    }
}

//发布电源电压话题
void BaseController::Publish_Voltage()
{
    auto voltage_msgs = std_msgs::msg::Float32();
    // std_msgs::Float32 voltage_msgs; //Define the data type of the power supply voltage publishing topic //定义电源电压发布话题的数据类型
    static float Count_Voltage_Pub=0;
    if(Count_Voltage_Pub++>10)
        {
        Count_Voltage_Pub=0;  
        voltage_msgs.data = serial_parser_->Power_voltage; //The power supply voltage is obtained //电源供电的电压获取
        voltage_pub_->publish(voltage_msgs); //Post the power supply voltage topic unit: V, volt //发布电源电压话题单位：V、伏特
        }
}


/**************************************
Function: 平方根倒数 求四元数用到
***************************************/
float InvSqrt(float number)
{
    volatile long i;
    volatile float x, y;
    volatile const float f = 1.5F;
    x = number * 0.5F;
    y = number;
    i = * (( long * ) &y);
    i = 0x5f375a86 - ( i >> 1 );
    y = * (( float * ) &i);
    y = y * ( f - ( x * y * y ) );

    return y;
}

/**************************************
Function: 四元数解算
***************************************/
volatile float twoKp = 1.0f;     // 2 * proportional gain (Kp)
volatile float twoKi = 0.0f;     // 2 * integral gain (Ki)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;          // quaternion of sensor frame relative to auxiliary frame
volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
geometry_msgs::msg::Quaternion Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az)
{
    geometry_msgs::msg::Quaternion msg;
    float recipNorm;
    float halfvx, halfvy, halfvz;
    float halfex, halfey, halfez;
    float qa, qb, qc;
    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
    // 首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模
    recipNorm = InvSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;      
    // 把四元数换算成方向余弦中的第三行的三个元素
    halfvx = q1 * q3 - q0 * q2;
    halfvy = q0 * q1 + q2 * q3;
    halfvz = q0 * q0 - 0.5f + q3 * q3;
    //误差是估计的重力方向和测量的重力方向的交叉乘积之和
    halfex = (ay * halfvz - az * halfvy);
    halfey = (az * halfvx - ax * halfvz);
    halfez = (ax * halfvy - ay * halfvx);
    // 计算并应用积分反馈（如果启用）
    if(twoKi > 0.0f) {
        integralFBx += twoKi * halfex * (1.0f / SAMPLING_FREQ);  // integral error scaled by Ki
        integralFBy += twoKi * halfey * (1.0f / SAMPLING_FREQ);
        integralFBz += twoKi * halfez * (1.0f / SAMPLING_FREQ);
        gx += integralFBx;        // apply integral feedback
        gy += integralFBy;
        gz += integralFBz;
    }
    else {
        integralFBx = 0.0f;       // prevent integral windup
        integralFBy = 0.0f;
        integralFBz = 0.0f;
    }
    // Apply proportional feedback
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
    }
    // Integrate rate of change of quaternion
    gx *= (0.5f * (1.0f / SAMPLING_FREQ));   // pre-multiply common factors
    gy *= (0.5f * (1.0f / SAMPLING_FREQ));
    gz *= (0.5f * (1.0f / SAMPLING_FREQ));
    qa = q0;
    qb = q1;
    qc = q2;
    q0 += (-qb * gx - qc * gy - q3 * gz);
    q1 += (qa * gx + qc * gz - q3 * gy);
    q2 += (qa * gy - qb * gz + q3 * gx);
    q3 += (qa * gz + qb * gy - qc * gx); 
    // Normalise quaternion
    recipNorm = InvSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
    msg.w = q0;
    msg.x = q1;
    msg.y = q2;
    msg.z = q3;
    return msg;
}

//发布IMU话题 
void BaseController::Publish_Imu(){

    sensor_msgs::msg::Imu imu_msg;
    // imu_msg.header.stamp = ros::Time::now(); 
    imu_msg.header.stamp = rclcpp::Node::now();
    imu_msg.header.frame_id = imu_frame_;
    //线性加速度单位转化，和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2
    imu_msg.linear_acceleration.x = serial_parser_->Imu_Data.accele_x_data / ACCEl_RATIO;
    imu_msg.linear_acceleration.y = serial_parser_->Imu_Data.accele_y_data / ACCEl_RATIO;
    imu_msg.linear_acceleration.z = serial_parser_->Imu_Data.accele_z_data / ACCEl_RATIO;

    //陀螺仪单位转化，和STM32的IMU初始化的时候的量程有关，这里IMU的陀螺仪的量程是±500°/s
    //因为机器人一般Z轴速度不快，降低量程可以提高精度
    imu_msg.angular_velocity.x =  serial_parser_->Imu_Data.gyros_x_data * GYROSCOPE_RATIO;
    imu_msg.angular_velocity.y =  serial_parser_->Imu_Data.gyros_y_data * GYROSCOPE_RATIO;
    imu_msg.angular_velocity.z =  serial_parser_->Imu_Data.gyros_z_data * GYROSCOPE_RATIO;
    imu_msg.angular_velocity_covariance[0] = 1e6; //三轴角速度协方差矩阵
    imu_msg.angular_velocity_covariance[4] = 1e6;
    imu_msg.angular_velocity_covariance[8] = 1e-6;

    // 通过IMU绕三轴角速度与三轴加速度计算三轴姿态
    imu_msg.orientation = Quaternion_Solution(imu_msg.angular_velocity.x, 
                                            imu_msg.angular_velocity.y, 
                                            imu_msg.angular_velocity.z,
                                            imu_msg.linear_acceleration.x, 
                                            imu_msg.linear_acceleration.y, 
                                            imu_msg.linear_acceleration.z);
    imu_msg.orientation_covariance[0] = 1e6;    //三轴姿态协方差矩阵
    imu_msg.orientation_covariance[4] = 1e6;
    imu_msg.orientation_covariance[8] = 1e-6;
    imu_pub_->publish(imu_msg); 

    // 使用IMU的角度作为yaw
    // tf2::Quaternion imu_quat(
    //   imu_msg.orientation.x,
    //   imu_msg.orientation.y,
    //   imu_msg.orientation.z,
    //   imu_msg.orientation.w);
    // double roll, pitch;
    // tf2::Matrix3x3 m(imu_quat);
    // m.getRPY(roll, pitch, robot_yaw_);
}

// 控制核心
void BaseController::ControlCore(){
    // time_last_ = ros::Time::now();
    // speed_time_last_ = ros::Time::now();
    time_last_ = rclcpp::Node::now();
    speed_time_last_ = rclcpp::Node::now();
    rclcpp::Node::SharedPtr node_(this);
    while(rclcpp::ok()){
        // if(serial_parser_ == nullptr) continue;
        // ROS_INFO("Sampling_Time: %0.5f",Sampling_Time);
        // time_now_ = ros::Time::now();
        time_now_ = rclcpp::Node::now();
        Sampling_Time = (time_now_ - time_last_).seconds(); 
        if(time_now_ > speed_time_last_ + rclcpp::Duration::from_seconds(0.5)){    //速度指令停止发送，机器立即停止
            if(serial_parser_ != nullptr)
                serial_parser_->SetMotorSpeed(0.0,0.0,0.0);      
        }
        if(serial_parser_->GetSensorData()) {
            //ROS_INFO("Get_Sensor_Data_New....");
            robot_x_+= (serial_parser_->Robot_Vel.X * cos(robot_yaw_) - serial_parser_->Robot_Vel.Y * sin(robot_yaw_)) * Sampling_Time;   //计算X方向的位移，单位：m
            robot_y_ += (serial_parser_->Robot_Vel.X * sin(robot_yaw_) + serial_parser_->Robot_Vel.Y * cos(robot_yaw_)) * Sampling_Time;  //计算Y方向的位移，单位：m
            robot_yaw_ += serial_parser_->Robot_Vel.Z * Sampling_Time; //绕Z轴的角位移，单位：rad 

            Publish_Odom();       //发布里程计话题
            Publish_Imu();        //发布IMU话题    
            Publish_Voltage();    //发布电源电压话题

            time_last_ = time_now_; 
        }
        // ros::spinOnce();  
        rclcpp::spin_some(node_);
    }
}
    
}   //base_controller